
//PS2  初始化
#include <PS2X_lib.h>  //for v1.6
/******************************************************************
   set pins connected to PS2 controller:
     - 1e column: original
     - 2e colmun: Stef?
   replace pin numbers by the ones you use
 ******************************************************************/
#define PS2_DAT        12  //14    
#define PS2_CMD        11  //15
#define PS2_SEL        10  //16
#define PS2_CLK        13  //17

/******************************************************************
   select modes of PS2 controller:
     - pressures = analog reading of push-butttons
     - rumble    = motor rumbling
   uncomment 1 of the lines for each mode selection
 ******************************************************************/
//#define pressures   true
#define pressures   false
//#define rumble      true
#define rumble      false

PS2X ps2x; // create PS2 Controller Class

//right now, the library does NOT support hot pluggable controllers, meaning
//you must always either restart your Arduino after you connect the controller,
//or call config_gamepad(pins) again after connecting the controller.

int error = 0;
byte type = 0;
byte vibrate = 0;

// Reset func
void (* resetFunc) (void) = 0;
//PS2  初始化

#include <Servo.h>                       //调用“Servo”库，调制无刷电机PWM输出
Servo ESC;                               //无刷电机命名为“ESC”
String throttle;                         //定义接收字符串为“throttle”
const int buzzer = 2;                    // 定义喇叭引脚为2号数字引脚
const int ledPin = 4;                  // 定义喇叭引脚为3号数字引脚
int mTime;
bool heart = 0;

void setup()                             //初始化参数
{
  ESC.attach(9);                        // 定义无刷电机引脚为9号数字引脚，可以输出PWM
  Serial.begin(9600);                  //设置串口通讯的波特率为9600

  ESC.writeMicroseconds(1000);            //无刷电机初始为低电平
  pinMode(ledPin, OUTPUT);             //前灯继电器输出
  pinMode(buzzer, OUTPUT);             //喇叭输出
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
  if (error == 0)
  {
    Serial.print("Found Controller, configured successful ");
    Serial.print("pressures = ");
    if (pressures)
      Serial.println("true ");
    else
      Serial.println("false");
    Serial.print("rumble = ");
    if (rumble)
      Serial.println("true)");
    else
      Serial.println("false");
    Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
    Serial.println("holding L1 or R1 will print out the analog stick values.");
    Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
  }
  else if (error == 1)
    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");

  else if (error == 2)
    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");

  else if (error == 3)
    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");



}

void Action(String throttle)//主调函数
{
  if (throttle == "stop")                  //暂停，速度减为零
  {
    Serial.println(" Stop");
    ESC.writeMicroseconds(1000);
    throttle = "0";
  }

  else if (throttle == "one")              //调节滑板速度为1
  {
    for (int i = 0; i <= 0; i++)
    {
      digitalWrite(buzzer, HIGH);
      delay(300);
      digitalWrite(buzzer, LOW);
    }
    int i = ESC.readMicroseconds();
    if (i <= 1350)
      for (i; i <= 1350; i++)
      {
        ESC.writeMicroseconds(i);
        delay(2);
      }
    else
      for (i; i >= 1350; i--)
      {
        ESC.writeMicroseconds(i);
        delay(2);
      }
    Serial.println(" speed 1");
    for (int i = 0; i <= 0; i++)
    {
      digitalWrite(buzzer, HIGH);
      delay(300);
      digitalWrite(buzzer, LOW);
    }
    throttle = "0";
  }

  else if (throttle == "two")              //调节滑板速度为2
  {
    int i = ESC.readMicroseconds();
    if (i <= 1550)
      for (i; i <= 1550; i++)
      {
        ESC.writeMicroseconds(i);
        delay(2);
      }
    else
      for (i; i >= 1550; i--)
      {
        ESC.writeMicroseconds(i);
        delay(2);
      }
    Serial.println(" speed 2");
    for (int i = 0; i <= 1; i++)
    {
      digitalWrite(buzzer, HIGH);
      delay(300);
      digitalWrite(buzzer, LOW);
      delay(200);
    }
    throttle = "0";
  }

  else if (throttle == "three")              //调节滑板速度为3
  {
    int i = ESC.readMicroseconds();
    if (i <= 1750)
      for (i; i <= 1750; i++)
      {
        ESC.writeMicroseconds(i);
        delay(2);
      }
    else
      for (i; i >= 1750; i--)
      {
        ESC.writeMicroseconds(i);
        delay(2);
      }
    Serial.println(" speed 3");
    for (int i = 0; i <= 2; i++)
    {
      digitalWrite(buzzer, HIGH);
      delay(300);
      digitalWrite(buzzer, LOW);
      delay(200);
    }
    throttle = "0";
  }

  else if (throttle == "four")              //调节滑板速度为4
  {
    int i = ESC.readMicroseconds();
    if (i <= 1950)
      for (i; i <= 1950; i++)
      {
        ESC.writeMicroseconds(i);
        delay(2);
      }
    else
      for (i; i >= 1950; i--)
      {
        ESC.writeMicroseconds(i);
        delay(2);
      }
    Serial.println(" speed 4");
    for (int i = 0; i <= 3; i++)
    {
      digitalWrite(buzzer, HIGH);
      delay(300);
      digitalWrite(buzzer, LOW);
      delay(200);
    }
    throttle = "0";
  }

  else if (throttle == "turn on")              //点亮前灯
  {
    digitalWrite(ledPin, HIGH);
    Serial.println(" turn on");
    digitalWrite(buzzer, HIGH);
    delay(500);
    digitalWrite(buzzer, LOW);
    throttle = "0";
  }

  else if (throttle == "turn off")              //熄灭前灯
  {
    digitalWrite(ledPin, LOW);
    Serial.println(" turn off");
    digitalWrite(buzzer, HIGH);
    delay(400);
    digitalWrite(buzzer, LOW);
    throttle = "0";
  }

  else if (throttle == "buzzer on")              //铃铛响
  {
    int times = 5;
    Serial.println(" buzzer on");
    for (int i = 0; i <= times; i++)
    {
      digitalWrite(buzzer, HIGH);
      delay(300);
      digitalWrite(buzzer, LOW);
      delay(200);
    }
    throttle = "0";
  }

  else if (throttle == "buzzer off")              //铃铛尖悦响
  {
    int times = 2;
    Serial.println(" buzzer off");
    for (int i = 0; i <= times; i++)
    {
      digitalWrite(buzzer, HIGH);
      delay(100);
      digitalWrite(buzzer, LOW);
      delay(200);
    }
    throttle = "0";
  }
}

void loop()                                       //主循环
{
//  ESC.writeMicroseconds(1000);
//  digitalWrite(buzzer, LOW);
//  digitalWrite(ledPin, LOW);

  if (Serial.available() > 0)                   //手机蓝牙遥控
  {
    throttle  = Serial.readString();            //单片机接收数据赋值给字符串“throttle”
    Serial.println(" 手机蓝牙模式");
    if (throttle == "start")
    {
      Serial.println(" Start,  ready go!");     //开始启动程序
      while (1)
      {
        throttle  = Serial.readString();
        if (throttle.equals(""))
        {
          heart = 1;
          mTime = millis();
        }

        Action(throttle);                         //发送调用函数自变量
        if (throttle == "reset")              //重新启动
        {
          Serial.println(" Reset");
          digitalWrite(buzzer, LOW);
          digitalWrite(ledPin, LOW);
          ESC.writeMicroseconds(1000);
          break;
        }
        if (heart == 1)                      //判断是否蓝牙断开，进而自保护
        {
          int test = millis();
          if (test - mTime > 10000)
          {
            ESC.writeMicroseconds(1000);
            digitalWrite(buzzer, LOW);
            digitalWrite(ledPin, LOW);
            Serial.println(" Reset");
            heart = 0;

          }
        }
      }
    }
    delay(3000);
    Serial.println(" Waiting...");
  }


  if (error == 1)
  { //skip loop if no controller found
    resetFunc();
  }

  else { //DualShock Controller
    ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed

    if (ps2x.ButtonPressed(PSB_START))        //will be TRUE as long as button is pressed
      Serial.println("Start is being held");
    if (ps2x.ButtonPressed(PSB_SELECT))
    {
      Serial.println("Reset");
      digitalWrite(buzzer, LOW);
      digitalWrite(ledPin, LOW);
      ESC.writeMicroseconds(1000);
    }

    if (ps2x.ButtonPressed(PSB_L3))
    {
      digitalWrite(ledPin, HIGH);
      Serial.println(" turn on");
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_R3))
    {
      digitalWrite(ledPin, LOW);
      Serial.println(" turn off");
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_L2))
    {
      int times = 5;
      Serial.println(" buzzer on");
      for (int i = 0; i <= times; i++)
      {
        digitalWrite(buzzer, HIGH);
        delay(300);
        digitalWrite(buzzer, LOW);
        delay(200);
      }
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_R2))
    {
      int times = 2;
      Serial.println(" buzzer off");
      for (int i = 0; i <= times; i++)
      {
        digitalWrite(buzzer, HIGH);
        delay(100);
        digitalWrite(buzzer, LOW);
        delay(200);
      }
      throttle = "0";
    }


    if (ps2x.ButtonPressed(PSB_CIRCLE))              //will be TRUE if button was JUST pressed
    {
      int i = ESC.readMicroseconds();
      if (i <= 1350)
        for (i; i <= 1350; i++)
        {
          ESC.writeMicroseconds(i);
          delay(2);
        }
      else
        for (i; i >= 1350; i--)
        {
          ESC.writeMicroseconds(i);
          delay(2);
        }
      Serial.println(" speed 1");
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_CROSS))              //will be TRUE if button was JUST pressed OR released
    {
      int i = ESC.readMicroseconds();
      if (i <= 1550)
        for (i; i <= 1550; i++)
        {
          ESC.writeMicroseconds(i);
          delay(2);
        }
      else
        for (i; i >= 1550; i--)
        {
          ESC.writeMicroseconds(i);
          delay(2);
        }
      Serial.println(" speed 2");
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_SQUARE))             //will be TRUE if button was JUST released
    {
      int i = ESC.readMicroseconds();
      if (i <= 1750)
        for (i; i <= 1750; i++)
        {
          ESC.writeMicroseconds(i);
          delay(2);
        }
      else
        for (i; i >= 1750; i--)
        {
          ESC.writeMicroseconds(i);
          delay(2);
        }
      Serial.println(" speed 3");
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_TRIANGLE))
    {
      int i = ESC.readMicroseconds();
      if (i <= 1950)
        for (i; i <= 1950; i++)
        {
          ESC.writeMicroseconds(i);
          delay(2);
        }
      else
        for (i; i >= 1950; i--)
        {
          ESC.writeMicroseconds(i);
          delay(2);
        }
      Serial.println(" speed 4");
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_L1) || ps2x.ButtonPressed(PSB_R1))
    { //print stick values if either is TRUE
      Serial.println(" stop");
      digitalWrite(buzzer, LOW);
      digitalWrite(ledPin, LOW);
      ESC.writeMicroseconds(1000);

    }
  }
  delay(50);
}



